//
// Created by lee on 24-3-20.
//

#ifndef DKS_AGX_STRUCTURE_AGX_SEND_H
#define DKS_AGX_STRUCTURE_AGX_SEND_H
#include <iomanip>


//// 枚举类型用于控制AGX发送过来的数据的解包过程，AGX发出
//typedef enum {
//    AGX_DECODE_HEAD1 = 0,
//    AGX_DECODE_HEAD2,
//    AGX_PAYLOAD_LEN,
//    AGX_DECODE_MSGID,
//    AGX_DECODE_PAYLOAD,
//    AGX_DECODE_CHECKSUM_L,      // 校验和低位
//    AGX_DECODE_CHECKSUM_H       // 校验和高位
//} agx_decode_state_t;



//结构体一字节对齐，结构体内是包的主要内容，不含头尾校验和，AGX发出
#pragma pack(push,1)
struct agx_to_px4_packet_t {
    uint8_t   node_id;         //消息来源于哪个节点 0x01-0xa0
    uint32_t  nav_time_2ms;   //2ms scale 本机时间，导航包计数
    uint8_t   nav_status;      //0x11:视觉-惯导; 0x22:惯导; 0x33:error
    int32_t   lon;        //e-7deg
    int32_t   lat;        //e-7deg
    int32_t   hgt;        //e-3m
    int32_t   roll_rate;      //e-4dps 弹体坐标系
    int32_t   pitch_rate;     //e-4dps
    int32_t   yaw_rate;       //e-4dps
    int32_t   roll;           //e-4deg 弹体坐标系与地理坐标系之间的夹角
    int32_t   pitch;          //e-4deg
    uint32_t  yaw;            //e-4deg
    int16_t   vel_e;          //e-2m/s  地理坐标系
    int16_t   vel_n;          //e-2m/s
    int16_t   vel_u;          //e-2m/s
    int16_t   vel;
    int32_t   accel_x;        //e-4m/s^2, absolute accel in body fram, not comtain gravity
    int32_t   accel_y;
    int32_t   accel_z;
    uint8_t   mission_state;    //0x11:prepare state; 0x22:investigation state;  0x33:follow state; 0x44:tmg state;
    int32_t   target_lon;        //e-7deg
    int32_t   target_lat;        //e-7deg
    int32_t   target_hgt;        //e-2m
    uint8_t   attack_call_flag;  //0x00 该节点不进行召唤  同节点ID：该节点召唤其他节点，进入跟踪阶段
    uint8_t   call_respond_flag; //0x00 该节点不进行响应  同节点ID：该节点响应召唤，进入跟踪阶段
    uint8_t   replanning_flag;   //0x11:no need replanning; 0x22:need to replan
    int32_t   waypoint_lon;        //e-7deg
    int32_t   waypoint_lat;        //e-7deg
    int32_t   waypoint_hgt;        //e-2m
    uint32_t  remaining_time_of_planing; //e-2s; 多弹同时进入末制导倒计时
    uint32_t  remaining_time_of_next_waypoint; //e-2s; 本机到下一航点倒计时
    int16_t   average_vel_to_next_waypoint;    //e-3m/s; 到下一航点的平均速度
    int16_t   target_pix_x;    //打击目标在图像上的坐标 左上为原点
    int16_t   target_pix_y;
    // 控制吊舱，软环仿真不用
    uint8_t PodCtrinfo;
    int16_t PodVarX;
    int16_t PodVarY;
    uint16_t PodVarZ;
    int8_t PodZoomVelocity;
} ;
#pragma pack(pop)


#define PX42AGX_MSG_ID                   0X07
#define AGX2PX4_MSG_ID                   0X06


//size_t  _rx_leng{0};
//size_t _rx_count{0};
//uint16_t _rx_ck{0};
//agx_decode_state_t _agx_decode_state{AGX_DECODE_HEAD1};
//
//uint16_t get_checksum(const uint8_t *buff, uint16_t sz) {
//    uint16_t check_sum = 0;
//    if (buff == nullptr) return 0;
//    for (int i = 0; i < sz ; i++ )
//    {
//        check_sum += buff[i];
//    }
//    return check_sum;
//}
//
//void addByteToChecksum(uint8_t b)
//{
//    _rx_ck += b;
//}
//
//void decodeInit()
//{
//    _rx_ck = 0;
//    _rx_count = 0;
//    _agx_decode_state = AGX_DECODE_HEAD1;
//}

//int parseChar(uint8_t b, uint8_t * packet, RECV_TYPE& recv_type){
//    int ret = 0;
//    switch (_agx_decode_state) {
//        case AGX_DECODE_HEAD1:
//            if (b == 0x55) {
//                _agx_decode_state = AGX_DECODE_HEAD2;
//            } else {
//                decodeInit();
//            }
//            break;
//
//        case AGX_DECODE_HEAD2:
//            if (b == 0xaa) {
//                _agx_decode_state = AGX_PAYLOAD_LEN;
//            } else {
//                decodeInit();
//            }
//            break;
//
//        case AGX_PAYLOAD_LEN:
//            if (b>0) {
////                printf("payload length is %d", b);
//                _agx_decode_state = AGX_DECODE_MSGID;
//                addByteToChecksum(b);
//                _rx_leng = b;
//            } else {
//                decodeInit();
//            }
//            break;
//
//        case AGX_DECODE_MSGID:
//            if (b == 0x7c) {
//                recv_type = agx_send_visual_inertial_nav_type;
//                addByteToChecksum(b);
//                _agx_decode_state = AGX_DECODE_PAYLOAD;
//            } else {
//                decodeInit();
//                printf("got wrong msgid, get %X\n",  b);
//            }
//            break;
//
//        case AGX_DECODE_PAYLOAD:
//            ((uint8_t *)(&packet))[_rx_count] = b;
//            _rx_count++;
//            addByteToChecksum(b);
//
//            if (_rx_count >= _rx_leng) {
//                _agx_decode_state = AGX_DECODE_CHECKSUM_L;
//            }
//            break;
//
//
//        case AGX_DECODE_CHECKSUM_L:
//            if (b == (_rx_ck & 0x00ff)) {
//                ret = 1;
//            } else {
//                ret = -1;
//            }
//            decodeInit();
//            break;
//
//        default:
//            decodeInit();
//            break;
//    }
//
//    return ret;  //ret = 1正确， -1错误， 0未进行到高位校验和
//}

#endif //DKS_AGX_STRUCTURE_AGX_SEND_H
